#include <math.h>
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>

#include "GL/glut.h"
#include "include.h"
#include "String.h"
#include "LineSegment.h"


String::String(Vector2D * anchor, StringSegment * segments, int numSegments)
{
	checkCollision = true;
	this->anchor = anchor;
	ownAnchor = false;
	this->segments = segments;
	this->numSegments = numSegments;
	restLength = 0.0;
	for(int i = 0; i < numSegments; i++)
		restLength += segments->restLength;
}

String::String(Vector2D * anchor, int numSegments, float mass, float length, float springConstant)
{
	checkCollision = true;
	this->anchor = anchor;
	ownAnchor = false;
	this->numSegments = numSegments;
	
	Vector2D currentPoint = *anchor;
	segments = (StringSegment *)malloc(sizeof(StringSegment) * numSegments);
	restLength = length * numSegments;
	for(int i = 0; i < numSegments; i++)
	{
		currentPoint = currentPoint + Vector2D(length,0);
		segments[i] = StringSegment(currentPoint, mass, length, springConstant);
	}
}
String::String(Vector2D * anchor, int numSegments, float mass, float length, float springConstant, bool ownAnchor)
{
	checkCollision = true;
	this->anchor = anchor;
	this->ownAnchor = ownAnchor;
	this->numSegments = numSegments;
	
	Vector2D currentPoint = *anchor;
	segments = (StringSegment *)malloc(sizeof(StringSegment) * numSegments);
	restLength = length * numSegments;
	for(int i = 0; i < numSegments; i++)
	{
		currentPoint = currentPoint + Vector2D(length,0);
		segments[i] = StringSegment(currentPoint, mass, length, springConstant);
	}
}
String::String(Vector2D * anchor, StringSegment * segments, int numSegments, bool ownAnchor)
{
	checkCollision = true;
	this->anchor = anchor;
	this->ownAnchor = ownAnchor;
	this->segments = segments;
	this->numSegments = numSegments;
	restLength = 0.0;
	for(int i = 0; i < numSegments; i++)
		restLength += segments->restLength;
}
void String::SetSpringConstant(float springConstant)
{
	segments->SetSpringConstant(springConstant);
}
void String::SetDampingConstant(float mass)
{
	segments->SetDampingConstant(mass);
}
String::~String()
{
	printf("Delete string\n");
	if(segments)
		delete(segments);
	segments = NULL;
	numSegments = 0;
	printf("Delete string3\n");
	if(ownAnchor && anchor)
		delete(anchor);
	anchor = NULL;
	printf("Delete string4\n");
}

Vector2D String::Think(double elapsedTime)
{
	Vector2D anchorForce = Vector2D(0,0);
	Vector2D lastPoint = *anchor;
	for(int i = 0; i < numSegments; i++)
	{
		Vector2D force = segments[i].CalculateStringForce(&lastPoint);
		segments[i].AddForce(force);
		if(i > 0)
		{
			segments[i-1].AddForce( -1.0 * force);
			force = segments[i].CalculateGravitationalForce(&lastPoint);
			segments[i - 1].AddForce(force);
		}
		else
		{
			anchorForce = anchorForce + ( -1.0 * force);
			force = segments[i].CalculateGravitationalForce(&lastPoint);
			anchorForce = anchorForce +(force);
		}
		lastPoint = segments[i].endPoint;
	}	
	
	for(int i = 0; i < numSegments; i++)
	{
		segments[i].Think(elapsedTime);
	}
	return anchorForce;
}
void String::AddGravitationalForce()
{
	for(int i = 0; i < numSegments; i++)
		segments[i].AddGravitationalForce();
}
void String::Draw()
{
	glColor3f(1,1,1);
	glDisable(GL_TEXTURE_2D);
	glPushMatrix();
	glBegin(GL_LINES);
	glVertex2f(anchor->x, anchor->y);
	glVertex2f(segments[0].endPoint.x, segments[0].endPoint.y);
	for(int i = 1; i < numSegments; i++)
	{
		glVertex2f(segments[i-1].endPoint.x, segments[i-1].endPoint.y);
		glVertex2f(segments[i].endPoint.x, segments[i].endPoint.y);
	}
	glEnd();
	glPopMatrix();
}
bool String::Collide(Object * object, Vector2D * location)
{
	//start at the tip and work our way back and see if any of the strings segments intersect any of the objects sides
	if(!checkCollision || !object->material->isGrabbable)
		return false;
		
	for(int i = numSegments - 1; i >= 0; i--)
	{
		LineSegment line;
		if(i > 0)
		{
			line = LineSegment(segments[i].endPoint, segments[i-1].endPoint);
		}
		else
		{
			line = LineSegment(segments[i].endPoint, *anchor);
		}
		//line = LineSegment(segments[numSegments - 1].endPoint, *anchor);
		if(line.Collide(object, location))
		{
			return true;
		}
		
	}
	return false;
}

bool String::Collide(ObjectList * objectList, Object * ignoreMe, Object ** hitObject, Vector2D * location)
{
		if(!checkCollision)
		return false;
		
	List<Object> * search = objectList->head;	
	Vector2D collisionPoint;
	float distance = 100000;
	float didCollide = false;
	while(search)
	{
		if(search->payload && search->payload != ignoreMe)
		{
			
			if(Collide(search->payload, &collisionPoint))
			{
				didCollide = true;
				float thisDist = 0.0;
				if((thisDist = segments[numSegments - 1].endPoint.dist(collisionPoint)) < distance)
				{
					distance = thisDist;
					*location = collisionPoint;
					*hitObject = search->payload;
				}
			}
		}
		search = search->next;
	}
	return didCollide;
}
bool String::Collide(ObjectList * objectList, Object ** hitObject, Vector2D * location)
{
	if(!checkCollision)
		return false;
		
	List<Object> * search = objectList->head;	
	Vector2D collisionPoint;
	float distance = 100000;
	float didCollide = false;
	while(search)
	{
		if(search->payload)
		{
			if(Collide(search->payload, &collisionPoint))
			{
				didCollide = true;
				float thisDist = 0.0;
				if((thisDist = segments[numSegments - 1].endPoint.dist(collisionPoint)) < distance)
				{
					distance = thisDist;
					*location = collisionPoint;
					*hitObject = search->payload;
				}
			}
		}
		search = search->next;
	}
	return didCollide;
}
float String::GetLength()
{
	return anchor->dist(segments[numSegments-1].endPoint);
}